#include "../inc/collisions.h"
#include "../inc/vec2D.h"

#include <math.h>
#include <iostream>

using namespace std;

Collisions::Collisions(Balle &_balle, Filet &_filet, Blob &_blob1, Blob &_blob2, const int _largeurT, const int _hauteurT) :
         lb1(_blob1.getLargeur()), lb2(_blob2.getLargeur()), hb1(_blob1.getHauteur()), hb2(_blob2.getHauteur()), rb(_balle.getRayon()),
         hauteurTerrain(_hauteurT), largeurTerrain(_largeurT), filet(_filet), balle(_balle)
{

}

Collisions::~Collisions()
{
}

float Collisions::distPoint(const vec2D &p1, const vec2D &p2)
{
    // AB = sqrt( (xb -xa)² + (yb - ya)²)
    return sqrt(pow(p2.x -p1.x,2) + pow(p2.y - p1.y,2));
}

float Collisions::dot(const vec2D &p1, const vec2D &p2)
{
    // dot(a,b) = ax *bx + ay*by
    return (p1.x * p2.x) + (p1.y * p2.y);
}

float Collisions::norme(const vec2D &p)
{
    // N(a) = sqrt(ax² + ay²)
    return sqrt(pow(p.x,2) + pow(p.y,2));
}

float Collisions::distSegment(const vec2D &p, const vec2D &s1,const vec2D &s2)
{
    if(dot(vec2D(s1,s2),vec2D(s1,p)) < 0)
        {return distPoint(p,s1); }

    if(dot(vec2D(s2,s1),vec2D(s2,p)) < 0)
        { return distPoint(p,s2);}

    float ah = dot(vec2D(s1,s2),vec2D(s1,p)) / norme(vec2D(s1,s2));

    return sqrt(pow( distPoint(s1,p),2) - pow(ah,2));

}


bool Collisions::isCollisionBalle(const vec2D &posBlob1, const vec2D &posBlob2, const vec2D &posBalle,const vec2D &posBalleSuivante, vec2D &normale, bool &solTouche, int &marqueur)
{

//    std::cout << posBalle.x << " " << posBalle.y << std::endl;
//    std::cout << posBalleSuivante.x << " " << posBalleSuivante.y << std::endl;

    //~ float x = posBalleSuivante.x - posBalle.x;
    vec2D nS= vec2D(posBalleSuivante.x - posBalle.x, posBalleSuivante.y - posBalle.y);

    nS.normalize();
//    std::cout << nS.x << " " << nS.y << std::endl << std::endl;


    /// Collisions avec le bord du terrain
    bool H = posBalle.y - rb < 0;
    bool G = posBalle.x - rb < 0;
    bool D = posBalle.x + rb > largeurTerrain;
    bool B = posBalle.y + rb > 0.9*hauteurTerrain;

    if(H && G)
    {
        if(nS.x>0 && nS.y>0) return false;
        normale = normalize(vec2D(1,1));
        return true;
    }

    if(H && D)
    {
        if(nS.x<0 && nS.y>0) return false;
        normale = normalize(vec2D(-1,1));
        return true;
    }

    if(B && G)
    {
        if(nS.x>0 && nS.y<0) return false;
        solTouche = true;
        marqueur = 1;
        normale = normalize(vec2D(1,-1));

        return true;
    }

    if(B && D)
    {
        if(nS.x<0 && nS.y<0) return false;
        solTouche = true;
        marqueur = 2;
        normale = normalize(vec2D(-1,-1));

        return true;
    }

    if(B) // collision de la balle avec la partie basse de l'écran
    {
        if(nS.x>0 && nS.y<0) return false;

        solTouche = true;
        if(posBalle.x < filet.ptBasGauche.x)
            marqueur = 1;
        if(posBalle.x > filet.ptBasDroit.x)
            marqueur = 2;

        normale = vec2D(0,-1);
        return true;
    }

    if(G) // collision de la balle avec la partie gauche de l'écran
    {
        if(nS.x>0 && nS.y>0) return false;

        normale = vec2D(1,0);
        return true;
    }
    if(D) // collision de la balle avec la partie droite de l'écran
    {
        if(nS.x<0 && nS.y>0) return false;

        normale = vec2D(-1,0);
        return true;
    }

    if(H) // collision de la balle avec la partie haute de l'écran
    {
        if(nS.x>0 && nS.y>0) return false;
        normale = vec2D(0,1);
        return true;
    }

    /// Collisions avec le filet

    float d_filetG = distSegment(posBalle, filet.ptBasGauche, filet.ptHautGauche);
    float d_filetD = distSegment(posBalle, filet.ptBasDroit, filet.ptHautDroit);
    float d_filetH = distSegment(posBalle, filet.ptHautGauche, filet.ptHautDroit);

    G = d_filetG < rb;
    D = d_filetD < rb;
    H = d_filetH < rb;

    if(G && D && H)
    {
        if(nS.x>0 && nS.y<0) return false;

        normale = vec2D(0,-1);
//        cout << "G D H " << d_filetG << " " << d_filetD << " " << d_filetH << endl;
        return true;
    }

    if(G && D)
    {
        if(nS.x>0 && nS.y<0) return false;
        normale = vec2D(0,-1);
//        cout << "G D " << d_filetG << " " << d_filetD << endl;
        return true;
    }

    if(G && H)
    {
        if(nS.x<0 && nS.y<0) return false;
        normale = normalize(vec2D(-1,-1));
//        cout << "G H " << d_filetG << " " << d_filetH << endl;
        return true;
    }
    if(D && H)
    {
        if(nS.x>0 && nS.y<0) return false;
        normale = normalize(vec2D(1,-1));
//        cout << "D H " << d_filetD << " " << d_filetH << endl;
        return true;
    }

    if(H)
    {
        if(nS.x>0 && nS.y<0) return false;
        normale = vec2D(0,-1);
//        cout << "H " << d_filetH<< endl;
        return true;
    }

    if(G)
    {
        if(nS.x<0 && nS.y>0) return false;
        normale = vec2D(-1,0);
//        cout << "G " << d_filetG<< endl;
        return true;
    }
    if(D)
    {
        if(nS.x>0 && nS.y>0) return false;
        normale = vec2D(1,0);
//        cout << "D " << d_filetD<< endl;
        return true;
    }

    /// Collisions avec le blob 1

//      hg---hd
//    mg        md
//    |          |
//    bg        bd
//     vbg -- vbd
    vec2D bg(posBlob1.x - lb1/2, posBlob1.y + hb1/2);
    vec2D bd(posBlob1.x + lb1/2, posBlob1.y + hb1/2);

    vec2D mg(posBlob1.x - lb1/2, posBlob1.y - hb1/4);
    vec2D md(posBlob1.x + lb1/2, posBlob1.y - hb1/4);

    vec2D hg(posBlob1.x - lb1/4, posBlob1.y - hb1/2);
    vec2D hd(posBlob1.x + lb1/4, posBlob1.y - hb1/2);

    float d_blob1G  = distSegment(posBalle, bg,mg);
    float d_blob1D  = distSegment(posBalle, bd,md);
    float d_blob1H  = distSegment(posBalle, hg,hd);
    float d_blob1MG = distSegment(posBalle, mg,hg);
    float d_blob1MD = distSegment(posBalle, md,hd);
    float d_blob1B  = distSegment(posBalle, bg,bd);

    G = d_blob1G < rb;
    D = d_blob1D < rb;
    H = d_blob1H < rb;
    B = d_blob1B < rb;

    bool MG = d_blob1MG < rb;
    bool MD = d_blob1MD < rb;

    if(G && D && H && MG && MD)
    {
      if(nS.y<0) return false;

//      cout << "G D H MG MD" << d_blob1G << " " << d_blob1D << " " << d_blob1H<< " " << d_blob1MG<< " " <<  d_blob1MD<< endl;
      normale = vec2D(0,-1);
      return true;
    }

    if(G && D && B)
    {
        if(nS.y>0) return false;

//      cout << "G D B" << d_blob1G << " " << d_blob1D << " " << d_blob1B << endl;
      normale = vec2D(0,1);
      return true;
    }

    if(MG && H && MD)
    {
        if(nS.y<0) return false;

//        cout << "MG H MD" << d_blob1MG << " "<< d_blob1H<< " " <<  d_blob1MG<< endl;
        normale = vec2D(0,-1);
        return true;
    }

    if(G && MG && H)
    {
        if(nS.x < 0 && nS.y<0) return false;

//        cout << "G MG H" << d_blob1G << " "<< d_blob1MG<< " " <<  d_blob1H<< endl;
        normale = normalize(vec2D(-1,-1));
        return true;
    }
    if(D && MD && H)
    {
        if(nS.x > 0 && nS.y<0) return false;

//        cout << "D MD H" << d_blob1D << " "<< d_blob1MD<< " " <<  d_blob1H<< endl;
        normale = normalize(vec2D(1,-1));
        return true;
    }

    if(G && MG)
    {
        if(nS.x < 0 && nS.y <0) return false;

      normale = normalize(vec2D(-1,-0.5));
      return true;
    }
    if(MG && H)
    {
        if(nS.x < 0 && nS.y <0) return false;

      normale = normalize(vec2D(-0.5,-1));
      return true;
    }
    if(D && MD)
    {
        if(nS.x > 0 && nS.y <0) return false;

      normale = normalize(vec2D(1,-0.5));
      return true;
    }

    if(MD && H)
    {
        if(nS.x > 0 && nS.y <0) return false;

      normale = normalize(vec2D(0.5,-1));
      return true;
    }
    if(B)
    {
        if(nS.y >0) return false;

      normale = normalize(vec2D(0,1));
      return true;
    }

    if(G)
    {
        if(nS.x < 0) return false;

      normale = vec2D(-1,0);
      return true;
    }
    if(D)
    {
        if(nS.x > 0 ) return false;

      normale = vec2D(1,0);
      return true;
    }
    if(H)
    {
        if(nS.y <0) return false;

      normale = vec2D(0,-1);
      return true;
    }

    /// Collisions avec le blob 2

    bg = vec2D(posBlob2.x - lb2/2, posBlob2.y + hb2/2);
    bd = vec2D(posBlob2.x + lb2/2, posBlob2.y + hb2/2);

    mg = vec2D(posBlob2.x - lb2/2, posBlob2.y - hb2/4);
    md = vec2D(posBlob2.x + lb2/2, posBlob2.y - hb2/4);

    hg = vec2D(posBlob2.x - lb2/4, posBlob2.y - hb2/2);
    hd = vec2D(posBlob2.x + lb2/4, posBlob2.y - hb2/2);

    float d_blob2G  = distSegment(posBalle, bg,mg);
    float d_blob2D  = distSegment(posBalle, bd,md);
    float d_blob2H  = distSegment(posBalle, hg,hd);
    float d_blob2MG = distSegment(posBalle, mg,hg);
    float d_blob2MD = distSegment(posBalle, md,hd);
    float d_blob2B  = distSegment(posBalle, bg,bd);

    G = d_blob2G < rb;
    D = d_blob2D < rb;
    MG = d_blob2MG < rb;
    MD = d_blob2MD < rb;
    H = d_blob2H < rb;
    B = d_blob2B < rb;


     if(G && D && H && MG && MD)
    {
      if(nS.y<0) return false;

//      cout << "G D H MG MD" << d_blob1G << " " << d_blob1D << " " << d_blob1H<< " " << d_blob1MG<< " " <<  d_blob1MD<< endl;
      normale = vec2D(0,-1);
      return true;
    }

    if(G && D && B)
    {
        if(nS.y>0) return false;

//      cout << "G D B" << d_blob1G << " " << d_blob1D << " " << d_blob1B << endl;
      normale = vec2D(0,1);
      return true;
    }

    if(MG && H && MD)
    {
        if(nS.y<0) return false;

//        cout << "MG H MD" << d_blob1MG << " "<< d_blob1H<< " " <<  d_blob1MG<< endl;
        normale = vec2D(0,-1);
        return true;
    }

    if(G && MG && H)
    {
        if(nS.x < 0 && nS.y<0) return false;

//        cout << "G MG H" << d_blob1G << " "<< d_blob1MG<< " " <<  d_blob1H<< endl;
        normale = normalize(vec2D(-1,-1));
        return true;
    }
    if(D && MD && H)
    {
        if(nS.x > 0 && nS.y<0) return false;

//        cout << "D MD H" << d_blob1D << " "<< d_blob1MD<< " " <<  d_blob1H<< endl;
        normale = normalize(vec2D(1,-1));
        return true;
    }

    if(G && MG)
    {
        if(nS.x < 0 && nS.y <0) return false;

      normale = normalize(vec2D(-1,-0.5));
      return true;
    }
    if(MG && H)
    {
        if(nS.x < 0 && nS.y <0) return false;

      normale = normalize(vec2D(-0.5,-1));
      return true;
    }
    if(D && MD)
    {
        if(nS.x > 0 && nS.y <0) return false;

      normale = normalize(vec2D(1,-0.5));
      return true;
    }

    if(MD && H)
    {
        if(nS.x > 0 && nS.y <0) return false;

      normale = normalize(vec2D(0.5,-1));
      return true;
    }
    if(B)
    {
        if(nS.y >0) return false;

      normale = normalize(vec2D(0,1));
      return true;
    }

    if(G)
    {
        if(nS.x < 0) return false;

      normale = vec2D(-1,0);
      return true;
    }
    if(D)
    {
        if(nS.x > 0 ) return false;

      normale = vec2D(1,0);
      return true;
    }
    if(H)
    {
        if(nS.y <0) return false;

      normale = vec2D(0,-1);
      return true;
    }
    return false;
}
